#include "stm32regs.h"
#include "core_cm.h"
#include "stm32regbit.h"

#define VECT_TAB_OFFSET 0x00

#if !defined(HSE_VALUE)
#define HSE_VALUE 8000000U /*!< Value of the External oscillator in Hz */
#endif                     /* HSE_VALUE */
#if !defined(HSI_VALUE)
#define HSI_VALUE 16000000U /*!< Value of the Internal oscillator in Hz*/
#endif                      /* HSI_VALUE */

uint32_t SystemCoreClock = HSI_VALUE;
const uint8_t AHBPrescTable[16] = {0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 1U, 2U, 3U, 4U, 6U, 7U, 8U, 9U};
const uint8_t APBPrescTable[8] = {0U, 0U, 0U, 0U, 1U, 2U, 3U, 4U};

void SystemCoreClockUpdate(void)
{
  uint32_t tmp, pllvco, pllr, pllsource, pllm;

  /* Get SYSCLK source -------------------------------------------------------*/
  switch (RCC->CFGR & RCC_CFGR_SWS)
  {
  case 0x04: /* HSI used as system clock source */
    SystemCoreClock = HSI_VALUE;
    break;

  case 0x08: /* HSE used as system clock source */
    SystemCoreClock = HSE_VALUE;
    break;

  case 0x0C: /* PLL used as system clock  source */
    /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLLM) * PLLN
         SYSCLK = PLL_VCO / PLLR
         */
    pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC);
    pllm = ((RCC->PLLCFGR & RCC_PLLCFGR_PLLM) >> 4) + 1U;
    if (pllsource == 0x02UL) /* HSI used as PLL clock source */
    {
      pllvco = (HSI_VALUE / pllm);
    }
    else /* HSE used as PLL clock source */
    {
      pllvco = (HSE_VALUE / pllm);
    }
    pllvco = pllvco * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 8);
    pllr = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLR) >> 25) + 1U) * 2U;
    SystemCoreClock = pllvco / pllr;
    break;

  default:
    break;
  }
  /* Compute HCLK clock frequency --------------------------------------------*/
  /* Get HCLK prescaler */
  tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)];
  /* HCLK clock frequency */
  SystemCoreClock >>= tmp;
}

void SystemInit(void)
{
/* FPU settings ------------------------------------------------------------*/
#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
  SCB->CPACR |= ((3UL << (10 * 2)) | (3UL << (11 * 2))); /* set CP10 and CP11 Full Access */
#endif

  /* Configure the Vector Table location add offset address ------------------*/
#ifdef VECT_TAB_SRAM
  SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */
#else
  SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */
#endif
  //SystemCoreClockUpdate();
}

void SystemClock_Config(void)
{
  MODIFY_REG(FLASH->ACR, FLASH_ACR_LATENCY, FLASH_ACR_LATENCY_8WS);
  SET_BIT(RCC->APB1ENR1, RCC_APB1ENR1_PWREN);
  READ_BIT(RCC->APB1ENR1, RCC_APB1ENR1_PWREN);
  CLEAR_BIT(PWR->CR5, PWR_CR5_R1MODE);
  CLEAR_BIT(RCC->APB1ENR1, RCC_APB1ENR1_PWREN);
  SET_BIT(RCC->CR, RCC_CR_HSEON);                                                  //打开外部高速时钟
  while (((READ_BIT(RCC->CR, RCC_CR_HSERDY) == (RCC_CR_HSERDY)) ? 1UL : 0UL) != 1) //等待启动
  {
  }
  MODIFY_REG(RCC->PLLCFGR, RCC_PLLCFGR_PLLSRC | RCC_PLLCFGR_PLLM | RCC_PLLCFGR_PLLN | RCC_PLLCFGR_PLLR,
             RCC_PLLCFGR_PLLSRC_HSE | (RCC_PLLCFGR_PLLM_2 | RCC_PLLCFGR_PLLM_0) | (85 << RCC_PLLCFGR_PLLN_Pos) | 0);
  MODIFY_REG(RCC->PLLCFGR, RCC_PLLCFGR_PLLSRC | RCC_PLLCFGR_PLLM | RCC_PLLCFGR_PLLN | RCC_PLLCFGR_PLLPDIV,
             RCC_PLLCFGR_PLLSRC_HSE | (RCC_PLLCFGR_PLLM_2 | RCC_PLLCFGR_PLLM_0) | (85 << RCC_PLLCFGR_PLLN_Pos) | (RCC_PLLCFGR_PLLPDIV_4 | RCC_PLLCFGR_PLLPDIV_3 | RCC_PLLCFGR_PLLPDIV_0));
  SET_BIT(RCC->PLLCFGR, RCC_PLLCFGR_PLLREN);
  SET_BIT(RCC->PLLCFGR, RCC_PLLCFGR_PLLPEN);

  SET_BIT(RCC->CR, RCC_CR_PLLON);
  /* Wait till PLL is ready */
  while (((READ_BIT(RCC->CR, RCC_CR_PLLRDY) == (RCC_CR_PLLRDY)) ? 1UL : 0UL) != 1)
  {
  }
  MODIFY_REG(RCC->CFGR, RCC_CFGR_SW, RCC_CFGR_SW_PLL);
  MODIFY_REG(RCC->CFGR, RCC_CFGR_HPRE, RCC_CFGR_HPRE_DIV2);
  while (((uint32_t)(READ_BIT(RCC->CFGR, RCC_CFGR_SWS))) != RCC_CFGR_SWS_PLL)
  {
  }

  //todo set CoreDebug->DEMCR
  //CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
  DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
  DWT->CYCCNT = 0;
  while (DWT->CYCCNT < 100)
    ;
}
